/*
File: caputure_pcl.cpp (adapted from pointcloud_online_viewer.cpp by rusu)
Usage: Capture point cloud from online data stream
Author: Qiao Zhao
Date: Jun 4th, 2011
*/
#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
// PCL includes
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl_visualization/pcl_visualizer.h>
#include <terminal_tools/print.h>
#include <terminal_tools/parse.h>

using namespace std;
using terminal_tools::print_highlight;
using terminal_tools::parse_argument;

typedef pcl::PointXYZRGB Point;
typedef pcl_visualization::PointCloudColorHandler<sensor_msgs::PointCloud2> ColorHandler;
typedef ColorHandler::Ptr ColorHandlerPtr;
typedef ColorHandler::ConstPtr ColorHandlerConstPtr;

sensor_msgs::PointCloud2ConstPtr cloud_, cloud_old_;
boost::mutex m;

void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
{
    //ROS_INFO ("PointCloud with %d data points (%s), stamp %f, and frame %s.", cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str ());
    m.lock ();
    cloud_ = cloud;
    m.unlock ();
}

/* ---[ */
int
main (int argc, char** argv)
{
    ros::init (argc, argv, "pointcloud_capture");
    ros::NodeHandle nh;
    int queue_size = 1;
    int nr_clouds = 1;
    ros::Subscriber sub = nh.subscribe ("camera/rgb/points", queue_size, cloud_cb);
    pcl_visualization::PCLVisualizer p ("PointCloud Viewer");
    pcl::PointCloud<Point> cloud_xyz;
    ColorHandlerPtr color_handler;
    pcl::PCDWriter writer;
    int color_handler_idx = 0;
    double psize = 0;
    int cloudcnt=0;
    char buf[50];
    while (nh.ok ())
    {
        // Spin
        ros::spinOnce ();
        ros::Duration (1).sleep ();
        p.spinOnce (10);
	
	
        // If no cloud received yet, continue
        if (!cloud_)
            continue;

        if (cloud_ == cloud_old_)
            continue;

        // Save the last color handler used
        //color_handler_idx = p.getColorHandlerIndex ("cloud");
	color_handler_idx=1;
        // Save the last point size used
        p.getPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, psize, "cloud");

        p.removePointCloud ("cloud");
        // Convert to PointCloud<T>
        m.lock ();

        {
            pcl::fromROSMsg (*cloud_, cloud_xyz);

            // Add a random color handler
            color_handler.reset (new pcl_visualization::PointCloudColorHandlerCustom<sensor_msgs::PointCloud2> (*cloud_, 255, 255, 255));
            p.addPointCloud (cloud_xyz, color_handler, "cloud");

            // Add a color handler for each field
            for (size_t i = 0; i < cloud_->fields.size (); ++i)
            {
                if (cloud_->fields[i].name == "rgb" || cloud_->fields[i].name == "rgba")
                    color_handler.reset (new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2> (*cloud_));
                else{
                    color_handler.reset (new pcl_visualization::PointCloudColorHandlerGenericField<sensor_msgs::PointCloud2> (*cloud_, cloud_->fields[i].name));
		    
		}
		printf("%s\n",cloud_->fields[i].name.c_str());
                p.addPointCloud (cloud_xyz, color_handler, "cloud");
            }

            // Set the point size
            p.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, psize, "cloud");

            // Set the old color handler index
            if (color_handler_idx != -1)
                p.updateColorHandlerIndex ("cloud", color_handler_idx);
            cloud_old_ = cloud_;
	    cloudcnt++;
	    sprintf(buf,"cloud/%dcloud.pcd",cloudcnt);
	    writer.write(buf,cloud_xyz,false);
        }
        m.unlock ();
    }

    return (0);
}